#ifndef pub_box_h
#define pub_box_h

#include <ros/ros.h>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>

#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>

#include <pcl/point_types.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/passthrough.h> // 直通滤波
#include <pcl/common/impl/common.hpp>

#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <message_filters/time_synchronizer.h>

#include "detect_msgs/Box.h"   // 自定义box消息
#include "detect_msgs/Boxes.h"

typedef pcl::PointXYZI PointT;

class ProcessBoxes
{
 private:
   ros::NodeHandle n_;
   std::string topic_boxes_;
   std::string topic_cloud_;
   float fx_, fy_, cx_, cy_;
   float k1_, k2_, k3_, p1_, p2_;
   float roll_, pitch_, yaw_;
   float x_, y_, z_;

   Eigen::Isometry3d Tcl_ = Eigen::Isometry3d::Identity();
  //  Eigen::Isometry3d K_ = Eigen::Isometry3d::Identity();

   pcl::PointCloud<PointT>::Ptr cloud_, cloud_voxel_;
   float filter_size_;
   bool is_filter_;

  std::string topic_boxes_new_;
  ros::Publisher pub_boxes_;
  detect_msgs::Box msg_box_;
  detect_msgs::Boxes msg_boxes_;

   message_filters::Subscriber<sensor_msgs::PointCloud2> sub_cloud_;
   message_filters::Subscriber<detect_msgs::Boxes> sub_boxes_;
   typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, detect_msgs::Boxes> MySyncPolicy;
   typedef message_filters::Synchronizer<MySyncPolicy> Sync;
   std::shared_ptr<Sync> sync_;
   
 public:
   ProcessBoxes();
   void getParam();
   void Rt2T(Eigen::Isometry3d& T);
   void callBack(const sensor_msgs::PointCloud2ConstPtr& msg_cloud, const detect_msgs::BoxesConstPtr& msg_boxes);
   void filterVoxel(const pcl::PointCloud<PointT>::Ptr& cloud_in, pcl::PointCloud<PointT>::Ptr& cloud_out, float size);
   void passThroughFilter( pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_in, const cv::Rect& box);

};

#endif